{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# MPPI (Model Predictive Path-Integral) Control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from typing import Tuple\n",
    "from matplotlib import patches\n",
    "from matplotlib.animation import ArtistAnimation\n",
    "from IPython import display"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Control Target : Vehicle\n",
    "\n",
    "<img src=\"../media/pathtracking.png\" width=\"500px\">"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Vehicle():\n",
    "    def __init__(\n",
    "            self,\n",
    "            wheel_base: float = 2.5, # [m]\n",
    "            vehicle_width = 3.0, # [m]\n",
    "            vehicle_length = 4.0, # [m]\n",
    "            max_steer_abs: float = 0.523, # [rad]\n",
    "            max_accel_abs: float = 2.000, # [m/s^2]\n",
    "            ref_path: np.ndarray = np.array([[-100.0, 0.0], [100.0, 0.0]]),\n",
    "            obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n",
    "            delta_t: float = 0.05, # [s]\n",
    "            visualize: bool = True,\n",
    "        ) -> None:\n",
    "        \"\"\"initialize vehicle environment\n",
    "        state variables:\n",
    "            x: x-axis position in the global frame [m]\n",
    "            y: y-axis position in the global frame [m]\n",
    "            yaw: orientation in the global frame [rad]\n",
    "            v: longitudinal velocity [m/s]\n",
    "        control input:\n",
    "            steer: front tire angle of the vehicle [rad] (positive in the counterclockwize direction)\n",
    "            accel: longitudinal acceleration of the vehicle [m/s^2] (positive in the forward direction)\n",
    "        Note: dynamics of the vehicle is the Kinematic Bicycle Model. \n",
    "        \"\"\"\n",
    "        # vehicle parameters\n",
    "        self.wheel_base = wheel_base#[m]\n",
    "        self.vehicle_w = vehicle_width\n",
    "        self.vehicle_l = vehicle_length\n",
    "        self.max_steer_abs = max_steer_abs # [rad]\n",
    "        self.max_accel_abs = max_accel_abs # [m/s^2]\n",
    "        self.delta_t = delta_t #[s]\n",
    "        self.ref_path = ref_path\n",
    "\n",
    "        # obstacle parameters\n",
    "        self.obstacle_circles = obstacle_circles\n",
    "\n",
    "        # visualization settings\n",
    "        self.view_x_lim_min, self.view_x_lim_max = -20.0, 20.0\n",
    "        self.view_y_lim_min, self.view_y_lim_max = -25.0, 25.0\n",
    "        self.minimap_view_x_lim_min, self.minimap_view_x_lim_max = -40.0, 40.0\n",
    "        self.minimap_view_y_lim_min, self.minimap_view_y_lim_max = -10.0, 40.0\n",
    "\n",
    "        # reset environment\n",
    "        self.visualize_flag = visualize\n",
    "        self.reset()\n",
    "\n",
    "    def reset(\n",
    "            self, \n",
    "            init_state: np.ndarray = np.array([0.0, 0.0, 0.0, 0.0]), # [x, y, yaw, v]\n",
    "        ) -> None:\n",
    "        \"\"\"reset environment to initial state\"\"\"\n",
    "\n",
    "        # reset state variables\n",
    "        self.state = init_state\n",
    "\n",
    "        # clear animation frames\n",
    "        self.frames = []\n",
    "\n",
    "        if self.visualize_flag:\n",
    "            # prepare figure\n",
    "            self.fig = plt.figure(figsize=(9,9))\n",
    "            self.main_ax = plt.subplot2grid((3,4), (0,0), rowspan=3, colspan=3)\n",
    "            self.minimap_ax = plt.subplot2grid((3,4), (0,3))\n",
    "            self.steer_ax = plt.subplot2grid((3,4), (1,3))\n",
    "            self.accel_ax = plt.subplot2grid((3,4), (2,3))\n",
    "\n",
    "            # graph layout settings\n",
    "            ## main view\n",
    "            self.main_ax.set_aspect('equal')\n",
    "            self.main_ax.set_xlim(self.view_x_lim_min, self.view_x_lim_max)\n",
    "            self.main_ax.set_ylim(self.view_y_lim_min, self.view_y_lim_max)\n",
    "            self.main_ax.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)\n",
    "            self.main_ax.tick_params(bottom=False, left=False, right=False, top=False)\n",
    "            ## mini map\n",
    "            self.minimap_ax.set_aspect('equal')\n",
    "            self.minimap_ax.axis('off')\n",
    "            self.minimap_ax.set_xlim(self.minimap_view_x_lim_min, self.minimap_view_x_lim_max)\n",
    "            self.minimap_ax.set_ylim(self.minimap_view_y_lim_min, self.minimap_view_y_lim_max)\n",
    "            ## steering angle view\n",
    "            self.steer_ax.set_title(\"Steering Angle\", fontsize=\"12\")\n",
    "            self.steer_ax.axis('off')\n",
    "            ## acceleration view\n",
    "            self.accel_ax.set_title(\"Acceleration\", fontsize=\"12\")\n",
    "            self.accel_ax.axis('off')\n",
    "            \n",
    "            # apply tight layout\n",
    "            self.fig.tight_layout()\n",
    "\n",
    "    def update(\n",
    "            self, \n",
    "            u: np.ndarray, \n",
    "            delta_t: float = 0.0, \n",
    "            append_frame: bool = True, \n",
    "            optimal_traj: np.ndarray = np.empty(0), # predicted optimal trajectory from mppi\n",
    "            sampled_traj_list: np.ndarray = np.empty(0), # sampled trajectories from mppi\n",
    "        ) -> None:\n",
    "        \"\"\"update state variables\"\"\"\n",
    "        # keep previous states\n",
    "        x, y, yaw, v = self.state\n",
    "\n",
    "        # prepare params\n",
    "        l = self.wheel_base\n",
    "        dt = self.delta_t if delta_t == 0.0 else delta_t\n",
    "\n",
    "        # limit control inputs\n",
    "        steer = np.clip(u[0], -self.max_steer_abs, self.max_steer_abs)\n",
    "        accel = np.clip(u[1], -self.max_accel_abs, self.max_accel_abs)\n",
    "\n",
    "        # update state variables\n",
    "        new_x = x + v * np.cos(yaw) * dt\n",
    "        new_y = y + v * np.sin(yaw) * dt\n",
    "        new_yaw = yaw + v / l * np.tan(steer) * dt\n",
    "        new_v = v + accel * dt\n",
    "        self.state = np.array([new_x, new_y, new_yaw, new_v])\n",
    "\n",
    "        # record frame\n",
    "        if append_frame:\n",
    "            self.append_frame(steer, accel, optimal_traj, sampled_traj_list)\n",
    "\n",
    "    def get_state(self) -> np.ndarray:\n",
    "        \"\"\"return state variables\"\"\"\n",
    "        return self.state.copy()\n",
    "\n",
    "    def append_frame(self, steer: float, accel: float, optimal_traj: np.ndarray, sampled_traj_list: np.ndarray) -> list:\n",
    "        \"\"\"draw a frame of the animation.\"\"\"\n",
    "        # get current states\n",
    "        x, y, yaw, v = self.state\n",
    "\n",
    "        ### main view ###\n",
    "        # draw the vehicle shape\n",
    "        vw, vl = self.vehicle_w, self.vehicle_l\n",
    "        vehicle_shape_x = [-0.5*vl, -0.5*vl, +0.5*vl, +0.5*vl, -0.5*vl, -0.5*vl]\n",
    "        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
    "        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
    "            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [0, 0]) # make the vehicle be at the center of the figure\n",
    "        frame = self.main_ax.plot(rotated_vehicle_shape_x, rotated_vehicle_shape_y, color='black', linewidth=2.0, zorder=3)\n",
    "\n",
    "        # draw wheels\n",
    "        ww, wl = 0.4, 0.7 #[m]\n",
    "        wheel_shape_x = np.array([-0.5*wl, -0.5*wl, +0.5*wl, +0.5*wl, -0.5*wl, -0.5*wl])\n",
    "        wheel_shape_y = np.array([0.0, +0.5*ww, +0.5*ww, -0.5*ww, -0.5*ww, 0.0])\n",
    "\n",
    "        ## rear-left wheel\n",
    "        wheel_shape_rl_x, wheel_shape_rl_y = \\\n",
    "            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, 0.3*vw])\n",
    "        wheel_rl_x, wheel_rl_y = \\\n",
    "            self._affine_transform(wheel_shape_rl_x, wheel_shape_rl_y, yaw, [0, 0])\n",
    "        frame += self.main_ax.fill(wheel_rl_x, wheel_rl_y, color='black', zorder=3)\n",
    "\n",
    "        ## rear-right wheel\n",
    "        wheel_shape_rr_x, wheel_shape_rr_y = \\\n",
    "            self._affine_transform(wheel_shape_x, wheel_shape_y, 0.0, [-0.3*vl, -0.3*vw])\n",
    "        wheel_rr_x, wheel_rr_y = \\\n",
    "            self._affine_transform(wheel_shape_rr_x, wheel_shape_rr_y, yaw, [0, 0])\n",
    "        frame += self.main_ax.fill(wheel_rr_x, wheel_rr_y, color='black', zorder=3)\n",
    "\n",
    "        ## front-left wheel\n",
    "        wheel_shape_fl_x, wheel_shape_fl_y = \\\n",
    "            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, 0.3*vw])\n",
    "        wheel_fl_x, wheel_fl_y = \\\n",
    "            self._affine_transform(wheel_shape_fl_x, wheel_shape_fl_y, yaw, [0, 0])\n",
    "        frame += self.main_ax.fill(wheel_fl_x, wheel_fl_y, color='black', zorder=3)\n",
    "\n",
    "        ## front-right wheel\n",
    "        wheel_shape_fr_x, wheel_shape_fr_y = \\\n",
    "            self._affine_transform(wheel_shape_x, wheel_shape_y, steer, [0.3*vl, -0.3*vw])\n",
    "        wheel_fr_x, wheel_fr_y = \\\n",
    "            self._affine_transform(wheel_shape_fr_x, wheel_shape_fr_y, yaw, [0, 0])\n",
    "        frame += self.main_ax.fill(wheel_fr_x, wheel_fr_y, color='black', zorder=3)\n",
    "\n",
    "        # draw the vehicle center circle\n",
    "        vehicle_center = patches.Circle([0, 0], radius=vw/20.0, fc='white', ec='black', linewidth=2.0, zorder=6)\n",
    "        frame += [self.main_ax.add_artist(vehicle_center)]\n",
    "\n",
    "        # draw the reference path\n",
    "        ref_path_x = self.ref_path[:, 0] - np.full(self.ref_path.shape[0], x)\n",
    "        ref_path_y = self.ref_path[:, 1] - np.full(self.ref_path.shape[0], y)\n",
    "        frame += self.main_ax.plot(ref_path_x, ref_path_y, color='black', linestyle=\"dashed\", linewidth=1.5)\n",
    "\n",
    "        # draw the information text\n",
    "        text = \"vehicle velocity = {v:>+6.1f} [m/s]\".format(pos_e=x, head_e=np.rad2deg(yaw), v=v)\n",
    "        frame += [self.main_ax.text(0.5, 0.02, text, ha='center', transform=self.main_ax.transAxes, fontsize=14, fontfamily='monospace')]\n",
    "\n",
    "        # draw the predicted optimal trajectory from mppi\n",
    "        if optimal_traj.any():\n",
    "            optimal_traj_x_offset = np.ravel(optimal_traj[:, 0]) - np.full(optimal_traj.shape[0], x)\n",
    "            optimal_traj_y_offset = np.ravel(optimal_traj[:, 1]) - np.full(optimal_traj.shape[0], y)\n",
    "            frame += self.main_ax.plot(optimal_traj_x_offset, optimal_traj_y_offset, color='#990099', linestyle=\"solid\", linewidth=1.5, zorder=5)\n",
    "\n",
    "        # draw the sampled trajectories from mppi\n",
    "        if sampled_traj_list.any():\n",
    "            min_alpha_value = 0.25\n",
    "            max_alpha_value = 0.35\n",
    "            for idx, sampled_traj in enumerate(sampled_traj_list):\n",
    "                # draw darker for better samples\n",
    "                alpha_value = (1.0 - (idx+1)/len(sampled_traj_list)) * (max_alpha_value - min_alpha_value) + min_alpha_value\n",
    "                sampled_traj_x_offset = np.ravel(sampled_traj[:, 0]) - np.full(sampled_traj.shape[0], x)\n",
    "                sampled_traj_y_offset = np.ravel(sampled_traj[:, 1]) - np.full(sampled_traj.shape[0], y)\n",
    "                frame += self.main_ax.plot(sampled_traj_x_offset, sampled_traj_y_offset, color='gray', linestyle=\"solid\", linewidth=0.2, zorder=4, alpha=alpha_value)\n",
    "\n",
    "        # draw the circular obstacles in the main view\n",
    "        for obs in self.obstacle_circles:\n",
    "            obs_x, obs_y, obs_r = obs\n",
    "            obs_circle = patches.Circle([obs_x-x, obs_y-y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n",
    "            frame += [self.main_ax.add_artist(obs_circle)]\n",
    "\n",
    "        ### mini map view ###\n",
    "        frame += self.minimap_ax.plot(self.ref_path[:, 0], self.ref_path[:,1], color='black', linestyle='dashed')\n",
    "        rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap = \\\n",
    "            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
    "        frame += self.minimap_ax.plot(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='black', linewidth=2.0, zorder=3)\n",
    "        frame += self.minimap_ax.fill(rotated_vehicle_shape_x_minimap, rotated_vehicle_shape_y_minimap, color='white', zorder=2)\n",
    "\n",
    "        # draw the circular obstacles in the mini map view\n",
    "        for obs in self.obstacle_circles:\n",
    "            obs_x, obs_y, obs_r = obs\n",
    "            obs_circle = patches.Circle([obs_x, obs_y], radius=obs_r, fc='white', ec='black', linewidth=2.0, zorder=0)\n",
    "            frame += [self.minimap_ax.add_artist(obs_circle)]\n",
    "\n",
    "        ### control input view ###\n",
    "        # steering angle\n",
    "        MAX_STEER = self.max_steer_abs\n",
    "        PIE_RATE = 3.0/4.0\n",
    "        PIE_STARTANGLE = 225 # [deg]\n",
    "        s_abs = np.abs(steer)\n",
    "        if steer < 0.0: # when turning right\n",
    "            steer_pie_obj, _ = self.steer_ax.pie([MAX_STEER*PIE_RATE, s_abs*PIE_RATE, (MAX_STEER-s_abs)*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
    "        else: # when turning left\n",
    "            steer_pie_obj, _ = self.steer_ax.pie([(MAX_STEER-s_abs)*PIE_RATE, s_abs*PIE_RATE, MAX_STEER*PIE_RATE, 2*MAX_STEER*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
    "        frame += steer_pie_obj\n",
    "        frame += [self.steer_ax.text(0, -1, f\"{np.rad2deg(steer):+.2f} \" + r\"$ \\rm{[deg]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
    "\n",
    "        # acceleration\n",
    "        MAX_ACCEL = self.max_accel_abs\n",
    "        PIE_RATE = 3.0/4.0\n",
    "        PIE_STARTANGLE = 225 # [deg]\n",
    "        a_abs = np.abs(accel)\n",
    "        if accel > 0.0:\n",
    "            accel_pie_obj, _ = self.accel_ax.pie([MAX_ACCEL*PIE_RATE, a_abs*PIE_RATE, (MAX_ACCEL-a_abs)*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
    "        else:\n",
    "            accel_pie_obj, _ = self.accel_ax.pie([(MAX_ACCEL-a_abs)*PIE_RATE, a_abs*PIE_RATE, MAX_ACCEL*PIE_RATE, 2*MAX_ACCEL*(1-PIE_RATE)], startangle=PIE_STARTANGLE, counterclock=False, colors=[\"lightgray\", \"black\", \"lightgray\", \"white\"], wedgeprops={'linewidth': 0, \"edgecolor\":\"white\", \"width\":0.4})\n",
    "        frame += accel_pie_obj\n",
    "        frame += [self.accel_ax.text(0, -1, f\"{accel:+.2f} \" + r\"$ \\rm{[m/s^2]}$\", size = 14, horizontalalignment='center', verticalalignment='center', fontfamily='monospace')]\n",
    "\n",
    "        # append frame\n",
    "        self.frames.append(frame)\n",
    "\n",
    "    # rotate shape and return location on the x-y plane.\n",
    "    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
    "        transformed_x = []\n",
    "        transformed_y = []\n",
    "        if len(xlist) != len(ylist):\n",
    "            print(\"[ERROR] xlist and ylist must have the same size.\")\n",
    "            raise AttributeError\n",
    "\n",
    "        for i, xval in enumerate(xlist):\n",
    "            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
    "            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
    "        transformed_x.append(transformed_x[0])\n",
    "        transformed_y.append(transformed_y[0])\n",
    "        return transformed_x, transformed_y\n",
    "\n",
    "    def show_animation(self, interval_ms: int) -> None:\n",
    "        \"\"\"show animation of the recorded frames\"\"\"\n",
    "        ani = ArtistAnimation(self.fig, self.frames, interval=interval_ms) # blit=True\n",
    "        html = display.HTML(ani.to_jshtml())\n",
    "        display.display(html)\n",
    "        plt.close()\n",
    "\n",
    "    def save_animation(self, filename: str, interval: int, movie_writer: str=\"ffmpeg\") -> None:\n",
    "        \"\"\"save animation of the recorded frames (ffmpeg required)\"\"\"\n",
    "        ani = ArtistAnimation(self.fig, self.frames, interval=interval)\n",
    "        ani.save(filename, writer=movie_writer)\n",
    "\n",
    "## [test simulation] ##\n",
    "sim_step = 100\n",
    "delta_t = 0.05\n",
    "ref_path = np.genfromtxt('../data/ovalpath.csv', delimiter=',', skip_header=1)\n",
    "vehicle = Vehicle(ref_path=ref_path[:, 0:2])\n",
    "for i in range(sim_step):\n",
    "    vehicle.update(u=[0.6 * np.sin(i/3.0), 2.2 * np.sin(i/10.0)], delta_t=delta_t) # u is the control input to the vehicle, [ steer[rad], accel[m/s^2] ]\n",
    "vehicle.show_animation(interval_ms=delta_t*1000) # show animation\n",
    "# save animation\n",
    "vehicle.save_animation(\"vehicle.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Controller : MPPI Controller\n",
    "\n",
    "### Note\n",
    "The following MPPI implementation follows Algorithms 1 and 2 of the reference paper. \n",
    "\n",
    "### Reference\n",
    "1. G. Williams et al. \"Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving\" \n",
    "    - URL : https://ieeexplore.ieee.org/document/8558663\n",
    "    - PDF : https://arxiv.org/pdf/1707.02342.pdf\n",
    "\n",
    "### Brief overview of MPPI algorithm\n",
    "Here is a general process flow to calculate optimal input with mppi algorithm.\n",
    "\n",
    "**[Step 1]** ramdomly sample input sequence\n",
    "\n",
    "Mean input sequence $U$ and ramdomly sampled input sequence $V$ are defined as follows.  \n",
    "Usually, optimal input sequence on the previous step is used as $U$. \n",
    "$$\n",
    "    \\begin{align}\n",
    "        & (\\mathbf{u}_0, \\mathbf{u}_1, ... \\mathbf{u}_{T-1}) = U \\in \\mathbb{R}^{m \\times T}, \\nonumber \\\\\n",
    "        & (\\mathbf{v}_0, \\mathbf{v}_1, ... \\mathbf{v}_{T-1}) = V \\in \\mathbb{R}^{m \\times T}, \\nonumber \\\\\n",
    "        & \\mathbf{v}_t = \\mathbf{u}_t + \\epsilon_t, \\nonumber \\\\\n",
    "        & \\epsilon_t \\sim \\mathcal{N}(0, \\Sigma).\\nonumber \n",
    "    \\end{align}\n",
    "$$\n",
    "\n",
    "\n",
    "**[Step 2]** predict future states and evaluate cost for each sample\n",
    "\n",
    "We assume a discrete time, continuous state-action dynamical system as a control target.  \n",
    "$\\mathbf{x}$ is a system state, and $\\mathbf{v}$ is a sampled control input.\n",
    "$$\n",
    "\\begin{align}\n",
    "\\mathbf{x}_t  &\\in \\mathbb{R}^{n}, \\nonumber \\\\\n",
    "\\mathbf{x}_{t+1} &= \\mathbf{F}(\\mathbf{x}_t, \\mathbf{v}_t).\\nonumber \n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "Then costs (i.e. penalties to be minimized) for sampled sequences $S(V; \\mathbf{x}_0)$ can be evaluated with following formulations.\n",
    "$$\n",
    "    \\begin{align}\n",
    "        & S(V; \\mathbf{x}_0) = C(\\mathcal{H}(V; \\mathbf{x}_0)), \\nonumber \\\\\n",
    "        & C(\\mathbf{x}_0, \\mathbf{x}_1, ... \\mathbf{x}_T) = \\phi(\\mathbf{x}_T) + \\sum_{t=0}^{T-1}c(\\mathbf{x}_t), \\nonumber \\\\\n",
    "        & \\mathcal{H}(V; \\mathbf{x}_0) = \\left( \\mathbf{x}_0, \\mathbf{F}(\\mathbf{x}_0, \\mathbf{v}_0), \\mathbf{F}(\\mathbf{F}(\\mathbf{x}_0, \\mathbf{v}_0), \\mathbf{v}_1), ... \\right).\\nonumber \n",
    "    \\end{align}\n",
    "$$\n",
    "\n",
    "**[Step 3]** calculate weight for each sample sequence\n",
    "\n",
    "Weight for a each sample sequence is derived on the basis of information theory.  \n",
    "There are K sample sequences in total, represented with an index k.  \n",
    "Good control sequence with small cost value get more weight, and vice versa.  \n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "& w(V) = \\frac{1}{\\eta} \\exp\n",
    "\\left( \n",
    "    -\\frac{1}{\\lambda}\n",
    "    \\left(\n",
    "        S(V) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t) - \\rho\n",
    "    \\right)\n",
    "\\right) \\nonumber \\\\\n",
    "& \\eta = \n",
    "\\sum_{k=1}^K \\exp\n",
    "\\left( \n",
    "    -\\frac{1}{\\lambda}\n",
    "    \\left(\n",
    "        S(U + \\mathcal{E}_k) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t^k) - \\rho\n",
    "    \\right)\n",
    "\\right)\\nonumber \\\\\n",
    "& \\rho = \n",
    "\\min_k \n",
    "\\left( S(V_k) + \\lambda(1-\\alpha) \\sum^{T-1}_{t=0} \\mathbf{u}_t^T \\Sigma^{-1} (\\mathbf{u}_t + \\epsilon_t^k) \\right)\\nonumber\n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "Note that $\\rho$ is inserted into the formulation to avoid overflow errors during implementation.\n",
    "\n",
    "**[Step 4]** get optimal control input sequence\n",
    "\n",
    "Finally, optimal input trajectory for the next ($i+1$) step is given adding weighted sample sequences to the previous solution.\n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "    \\mathbf{u}_t^{i+1} % &= \\mathbb{E}_{\\mathbb{Q}_{\\hat{U}, \\Sigma}}[w(V)\\mathbf{v}_t]\n",
    "                 = u_t^i + \\sum_{k=1}^K w(V_k) \\epsilon_t^k \\nonumber \n",
    "\\end{align}\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class MPPIControllerForPathTracking():\n",
    "    def __init__(\n",
    "            self,\n",
    "            delta_t: float = 0.05,\n",
    "            wheel_base: float = 2.5, # [m]\n",
    "            vehicle_width: float = 3.0, # [m]\n",
    "            vehicle_length: float = 4.0, # [m]\n",
    "            max_steer_abs: float = 0.523, # [rad]\n",
    "            max_accel_abs: float = 2.000, # [m/s^2]\n",
    "            ref_path: np.ndarray = np.array([[0.0, 0.0, 0.0, 1.0], [10.0, 0.0, 0.0, 1.0]]),\n",
    "            horizon_step_T: int = 30,\n",
    "            number_of_samples_K: int = 1000,\n",
    "            param_exploration: float = 0.0,\n",
    "            param_lambda: float = 50.0,\n",
    "            param_alpha: float = 1.0,\n",
    "            sigma: np.ndarray = np.array([[0.5, 0.0], [0.0, 0.1]]), \n",
    "            stage_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
    "            terminal_cost_weight: np.ndarray = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
    "            visualize_optimal_traj = True,  # if True, optimal trajectory is visualized\n",
    "            visualze_sampled_trajs = False, # if True, sampled trajectories are visualized\n",
    "            obstacle_circles: np.ndarray = np.array([[-2.0, 1.0, 1.0], [2.0, -1.0, 1.0]]), # [obs_x, obs_y, obs_radius]\n",
    "            collision_safety_margin_rate: float = 1.2, # safety margin for collision check\n",
    "    ) -> None:\n",
    "        \"\"\"initialize mppi controller for path-tracking\"\"\"\n",
    "        # mppi parameters\n",
    "        self.dim_x = 4 # dimension of system state vector\n",
    "        self.dim_u = 2 # dimension of control input vector\n",
    "        self.T = horizon_step_T # prediction horizon\n",
    "        self.K = number_of_samples_K # number of sample trajectories\n",
    "        self.param_exploration = param_exploration  # constant parameter of mppi\n",
    "        self.param_lambda = param_lambda  # constant parameter of mppi\n",
    "        self.param_alpha = param_alpha # constant parameter of mppi\n",
    "        self.param_gamma = self.param_lambda * (1.0 - (self.param_alpha))  # constant parameter of mppi\n",
    "        self.Sigma = sigma # deviation of noise\n",
    "        self.stage_cost_weight = stage_cost_weight\n",
    "        self.terminal_cost_weight = terminal_cost_weight\n",
    "        self.visualize_optimal_traj = visualize_optimal_traj\n",
    "        self.visualze_sampled_trajs = visualze_sampled_trajs\n",
    "\n",
    "        # vehicle parameters\n",
    "        self.delta_t = delta_t #[s]\n",
    "        self.wheel_base = wheel_base #[m]\n",
    "        self.vehicle_w = vehicle_width #[m]\n",
    "        self.vehicle_l = vehicle_length #[m]\n",
    "        self.max_steer_abs = max_steer_abs # [rad]\n",
    "        self.max_accel_abs = max_accel_abs # [m/s^2]\n",
    "        self.ref_path = ref_path\n",
    "\n",
    "        # obstacle parameters\n",
    "        self.obstacle_circles = obstacle_circles\n",
    "        self.collision_safety_margin_rate = collision_safety_margin_rate\n",
    "\n",
    "        # mppi variables\n",
    "        self.u_prev = np.zeros((self.T, self.dim_u))\n",
    "\n",
    "        # ref_path info\n",
    "        self.prev_waypoints_idx = 0\n",
    "\n",
    "    def calc_control_input(self, observed_x: np.ndarray) -> Tuple[float, np.ndarray]:\n",
    "        \"\"\"calculate optimal control input\"\"\"\n",
    "        # load privious control input sequence\n",
    "        u = self.u_prev\n",
    "\n",
    "        # set initial x value from observation\n",
    "        x0 = observed_x\n",
    "\n",
    "        # get the waypoint closest to current vehicle position \n",
    "        self._get_nearest_waypoint(x0[0], x0[1], update_prev_idx=True)\n",
    "        if self.prev_waypoints_idx >= self.ref_path.shape[0]-1:\n",
    "            print(\"[ERROR] Reached the end of the reference path.\")\n",
    "            raise IndexError\n",
    "\n",
    "        # prepare buffer\n",
    "        S = np.zeros((self.K)) # state cost list\n",
    "\n",
    "        # sample noise\n",
    "        epsilon = self._calc_epsilon(self.Sigma, self.K, self.T, self.dim_u) # size is self.K x self.T\n",
    "\n",
    "        # prepare buffer of sampled control input sequence\n",
    "        v = np.zeros((self.K, self.T, self.dim_u)) # control input sequence with noise\n",
    "\n",
    "        # loop for 0 ~ K-1 samples\n",
    "        for k in range(self.K):         \n",
    "\n",
    "            # set initial(t=0) state x i.e. observed state of the vehicle\n",
    "            x = x0\n",
    "\n",
    "            # loop for time step t = 1 ~ T\n",
    "            for t in range(1, self.T+1):\n",
    "\n",
    "                # get control input with noise\n",
    "                if k < (1.0-self.param_exploration)*self.K:\n",
    "                    v[k, t-1] = u[t-1] + epsilon[k, t-1] # sampling for exploitation\n",
    "                else:\n",
    "                    v[k, t-1] = epsilon[k, t-1] # sampling for exploration\n",
    "\n",
    "                # update x\n",
    "                x = self._F(x, self._g(v[k, t-1]))\n",
    "\n",
    "                # add stage cost\n",
    "                S[k] += self._c(x) + self.param_gamma * u[t-1].T @ np.linalg.inv(self.Sigma) @ v[k, t-1]\n",
    "\n",
    "            # add terminal cost\n",
    "            S[k] += self._phi(x)\n",
    "\n",
    "        # compute information theoretic weights for each sample\n",
    "        w = self._compute_weights(S)\n",
    "\n",
    "        # calculate w_k * epsilon_k\n",
    "        w_epsilon = np.zeros((self.T, self.dim_u))\n",
    "        for t in range(0, self.T): # loop for time step t = 0 ~ T-1\n",
    "            for k in range(self.K):\n",
    "                w_epsilon[t] += w[k] * epsilon[k, t]\n",
    "\n",
    "        # apply moving average filter for smoothing input sequence\n",
    "        w_epsilon = self._moving_average_filter(xx=w_epsilon, window_size=10)\n",
    "\n",
    "        # update control input sequence\n",
    "        u += w_epsilon\n",
    "\n",
    "        # calculate optimal trajectory\n",
    "        optimal_traj = np.zeros((self.T, self.dim_x))\n",
    "        if self.visualize_optimal_traj:\n",
    "            x = x0\n",
    "            for t in range(0, self.T): # loop for time step t = 0 ~ T-1\n",
    "                x = self._F(x, self._g(u[t]))\n",
    "                optimal_traj[t] = x\n",
    "\n",
    "        # calculate sampled trajectories\n",
    "        sampled_traj_list = np.zeros((self.K, self.T, self.dim_x))\n",
    "        sorted_idx = np.argsort(S) # sort samples by state cost, 0th is the best sample\n",
    "        if self.visualze_sampled_trajs:\n",
    "            for k in sorted_idx:\n",
    "                x = x0\n",
    "                for t in range(0, self.T): # loop for time step t = 0 ~ T-1\n",
    "                    x = self._F(x, self._g(v[k, t]))\n",
    "                    sampled_traj_list[k, t] = x\n",
    "\n",
    "        # update privious control input sequence (shift 1 step to the left)\n",
    "        self.u_prev[:-1] = u[1:]\n",
    "        self.u_prev[-1] = u[-1]\n",
    "\n",
    "        # return optimal control input and input sequence\n",
    "        return u[0], u, optimal_traj, sampled_traj_list\n",
    "\n",
    "    def _calc_epsilon(self, sigma: np.ndarray, size_sample: int, size_time_step: int, size_dim_u: int) -> np.ndarray:\n",
    "        \"\"\"sample epsilon\"\"\"\n",
    "        # check if sigma row size == sigma col size == size_dim_u and size_dim_u > 0\n",
    "        if sigma.shape[0] != sigma.shape[1] or sigma.shape[0] != size_dim_u or size_dim_u < 1:\n",
    "            print(\"[ERROR] sigma must be a square matrix with the size of size_dim_u.\")\n",
    "            raise ValueError\n",
    "\n",
    "        # sample epsilon\n",
    "        mu = np.zeros((size_dim_u)) # set average as a zero vector\n",
    "        epsilon = np.random.multivariate_normal(mu, sigma, (size_sample, size_time_step))\n",
    "        return epsilon\n",
    "\n",
    "    def _g(self, v: np.ndarray) -> float:\n",
    "        \"\"\"clamp input\"\"\"\n",
    "        # limit control inputs\n",
    "        v[0] = np.clip(v[0], -self.max_steer_abs, self.max_steer_abs) # limit steering input\n",
    "        v[1] = np.clip(v[1], -self.max_accel_abs, self.max_accel_abs) # limit acceleraiton input\n",
    "        return v\n",
    "\n",
    "    def _c(self, x_t: np.ndarray) -> float:\n",
    "        \"\"\"calculate stage cost\"\"\"\n",
    "        # parse x_t\n",
    "        x, y, yaw, v = x_t\n",
    "        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n",
    "\n",
    "        # calculate stage cost\n",
    "        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n",
    "        stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \\\n",
    "                     self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2\n",
    "        \n",
    "        # add penalty for collision with obstacles\n",
    "        stage_cost += self._is_collided(x_t) * 1.0e10\n",
    "\n",
    "        return stage_cost\n",
    "\n",
    "    def _phi(self, x_T: np.ndarray) -> float:\n",
    "        \"\"\"calculate terminal cost\"\"\"\n",
    "        # parse x_T\n",
    "        x, y, yaw, v = x_T\n",
    "        yaw = ((yaw + 2.0*np.pi) % (2.0*np.pi)) # normalize theta to [0, 2*pi]\n",
    "\n",
    "        # calculate terminal cost\n",
    "        _, ref_x, ref_y, ref_yaw, ref_v = self._get_nearest_waypoint(x, y)\n",
    "        terminal_cost = self.terminal_cost_weight[0]*(x-ref_x)**2 + self.terminal_cost_weight[1]*(y-ref_y)**2 + \\\n",
    "                        self.terminal_cost_weight[2]*(yaw-ref_yaw)**2 + self.terminal_cost_weight[3]*(v-ref_v)**2\n",
    "        \n",
    "        # add penalty for collision with obstacles\n",
    "        terminal_cost += self._is_collided(x_T) * 1.0e10\n",
    "        \n",
    "        return terminal_cost\n",
    "\n",
    "    def _is_collided(self,  x_t: np.ndarray) -> bool:\n",
    "        \"\"\"check if the vehicle is collided with obstacles\"\"\"\n",
    "        # vehicle shape parameters\n",
    "        vw, vl = self.vehicle_w, self.vehicle_l\n",
    "        safety_margin_rate = self.collision_safety_margin_rate\n",
    "        vw, vl = vw*safety_margin_rate, vl*safety_margin_rate\n",
    "\n",
    "        # get current states\n",
    "        x, y, yaw, _ = x_t\n",
    "\n",
    "        # key points for collision check\n",
    "        vehicle_shape_x = [-0.5*vl, -0.5*vl, 0.0, +0.5*vl, +0.5*vl, +0.5*vl, 0.0, -0.5*vl, -0.5*vl]\n",
    "        vehicle_shape_y = [0.0, +0.5*vw, +0.5*vw, +0.5*vw, 0.0, -0.5*vw, -0.5*vw, -0.5*vw, 0.0]\n",
    "        rotated_vehicle_shape_x, rotated_vehicle_shape_y = \\\n",
    "            self._affine_transform(vehicle_shape_x, vehicle_shape_y, yaw, [x, y]) # make the vehicle be at the center of the figure\n",
    "\n",
    "        # check if the key points are inside the obstacles\n",
    "        for obs in self.obstacle_circles: # for each circular obstacles\n",
    "            obs_x, obs_y, obs_r = obs # [m] x, y, radius\n",
    "            for p in range(len(rotated_vehicle_shape_x)):\n",
    "                if (rotated_vehicle_shape_x[p]-obs_x)**2 + (rotated_vehicle_shape_y[p]-obs_y)**2 < obs_r**2:\n",
    "                    return 1.0 # collided\n",
    "\n",
    "        return 0.0 # not collided\n",
    "\n",
    "    # rotate shape and return location on the x-y plane.\n",
    "    def _affine_transform(self, xlist: list, ylist: list, angle: float, translation: list=[0.0, 0.0]) -> Tuple[list, list]:\n",
    "        transformed_x = []\n",
    "        transformed_y = []\n",
    "        if len(xlist) != len(ylist):\n",
    "            print(\"[ERROR] xlist and ylist must have the same size.\")\n",
    "            raise AttributeError\n",
    "\n",
    "        for i, xval in enumerate(xlist):\n",
    "            transformed_x.append((xlist[i])*np.cos(angle)-(ylist[i])*np.sin(angle)+translation[0])\n",
    "            transformed_y.append((xlist[i])*np.sin(angle)+(ylist[i])*np.cos(angle)+translation[1])\n",
    "        transformed_x.append(transformed_x[0])\n",
    "        transformed_y.append(transformed_y[0])\n",
    "        return transformed_x, transformed_y\n",
    "\n",
    "    def _get_nearest_waypoint(self, x: float, y: float, update_prev_idx: bool = False):\n",
    "        \"\"\"search the closest waypoint to the vehicle on the reference path\"\"\"\n",
    "\n",
    "        SEARCH_IDX_LEN = 200 # [points] forward search range\n",
    "        prev_idx = self.prev_waypoints_idx\n",
    "        dx = [x - ref_x for ref_x in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 0]]\n",
    "        dy = [y - ref_y for ref_y in self.ref_path[prev_idx:(prev_idx + SEARCH_IDX_LEN), 1]]\n",
    "        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]\n",
    "        min_d = min(d)\n",
    "        nearest_idx = d.index(min_d) + prev_idx\n",
    "\n",
    "        # get reference values of the nearest waypoint\n",
    "        ref_x = self.ref_path[nearest_idx,0]\n",
    "        ref_y = self.ref_path[nearest_idx,1]\n",
    "        ref_yaw = self.ref_path[nearest_idx,2]\n",
    "        ref_v = self.ref_path[nearest_idx,3]\n",
    "\n",
    "        # update nearest waypoint index if necessary\n",
    "        if update_prev_idx:\n",
    "            self.prev_waypoints_idx = nearest_idx \n",
    "\n",
    "        return nearest_idx, ref_x, ref_y, ref_yaw, ref_v\n",
    "\n",
    "    def _F(self, x_t: np.ndarray, v_t: np.ndarray) -> np.ndarray:\n",
    "        \"\"\"calculate next state of the vehicle\"\"\"\n",
    "        # get previous state variables\n",
    "        x, y, yaw, v = x_t\n",
    "        steer, accel = v_t\n",
    "\n",
    "        # prepare params\n",
    "        l = self.wheel_base\n",
    "        dt = self.delta_t\n",
    "\n",
    "        # update state variables\n",
    "        new_x = x + v * np.cos(yaw) * dt\n",
    "        new_y = y + v * np.sin(yaw) * dt\n",
    "        new_yaw = yaw + v / l * np.tan(steer) * dt\n",
    "        new_v = v + accel * dt\n",
    "\n",
    "        # return updated state\n",
    "        x_t_plus_1 = np.array([new_x, new_y, new_yaw, new_v])\n",
    "        return x_t_plus_1\n",
    "\n",
    "    def _compute_weights(self, S: np.ndarray) -> np.ndarray:\n",
    "        \"\"\"compute weights for each sample\"\"\"\n",
    "        # prepare buffer\n",
    "        w = np.zeros((self.K))\n",
    "\n",
    "        # calculate rho\n",
    "        rho = S.min()\n",
    "\n",
    "        # calculate eta\n",
    "        eta = 0.0\n",
    "        for k in range(self.K):\n",
    "            eta += np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\n",
    "\n",
    "        # calculate weight\n",
    "        for k in range(self.K):\n",
    "            w[k] = (1.0 / eta) * np.exp( (-1.0/self.param_lambda) * (S[k]-rho) )\n",
    "        return w\n",
    "\n",
    "    def _moving_average_filter(self, xx: np.ndarray, window_size: int) -> np.ndarray:\n",
    "        \"\"\"apply moving average filter for smoothing input sequence\n",
    "        Ref. https://zenn.dev/bluepost/articles/1b7b580ab54e95\n",
    "        Note: The original MPPI paper uses the Savitzky-Golay Filter for smoothing control inputs.\n",
    "        \"\"\"\n",
    "        b = np.ones(window_size)/window_size\n",
    "        dim = xx.shape[1]\n",
    "        xx_mean = np.zeros(xx.shape)\n",
    "\n",
    "        for d in range(dim):\n",
    "            xx_mean[:,d] = np.convolve(xx[:,d], b, mode=\"same\")\n",
    "            n_conv = math.ceil(window_size/2)\n",
    "            xx_mean[0,d] *= window_size/n_conv\n",
    "            for i in range(1, n_conv):\n",
    "                xx_mean[i,d] *= window_size/(i+n_conv)\n",
    "                xx_mean[-i,d] *= window_size/(i + n_conv - (window_size % 2)) \n",
    "        return xx_mean\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# simulation settings\n",
    "delta_t = 0.05 # [sec]\n",
    "sim_steps = 150 # [steps]\n",
    "print(f\"[INFO] delta_t : {delta_t:.2f}[s] , sim_steps : {sim_steps}[steps], total_sim_time : {delta_t*sim_steps:.2f}[s]\")\n",
    "\n",
    "# obstacle params\n",
    "OBSTACLE_CIRCLES = np.array([\n",
    "    [+ 8.0, +5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n",
    "    [+18.0, -5.0, 4.0], # pos_x, pos_y, radius [m] in the global frame\n",
    "])\n",
    "\n",
    "# load and visualize reference path\n",
    "ref_path = np.genfromtxt('../data/ovalpath.csv', delimiter=',', skip_header=1)\n",
    "plt.title(\"Reference Path\")\n",
    "plt.plot(ref_path[:,0], ref_path[:,1])\n",
    "plt.show()\n",
    "\n",
    "# initialize a vehicle as a control target\n",
    "vehicle = Vehicle(\n",
    "    wheel_base=2.5,\n",
    "    max_steer_abs=0.523, # [rad]\n",
    "    max_accel_abs=2.000, # [m/s^2]\n",
    "    ref_path = ref_path[:, 0:2], # ndarray, size is <num_of_waypoints x 2>\n",
    "    obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n",
    ")\n",
    "vehicle.reset(\n",
    "    init_state = np.array([0.0, 0.0, 0.0, 0.0]), # [x[m], y[m], yaw[rad], v[m/s]]\n",
    ")\n",
    "\n",
    "# initialize a mppi controller for the vehicle\n",
    "mppi = MPPIControllerForPathTracking(\n",
    "    delta_t = delta_t*2.0, # [s]\n",
    "    wheel_base = 2.5, # [m]\n",
    "    max_steer_abs = 0.523, # [rad]\n",
    "    max_accel_abs = 2.000, # [m/s^2]\n",
    "    ref_path = ref_path, # ndarray, size is <num_of_waypoints x 2>\n",
    "    horizon_step_T = 20, # [steps]\n",
    "    number_of_samples_K = 500, # [samples]\n",
    "    param_exploration = 0.05,\n",
    "    param_lambda = 100.0,\n",
    "    param_alpha = 0.98,\n",
    "    sigma = np.array([[0.075, 0.0], [0.0, 2.0]]),\n",
    "    stage_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
    "    terminal_cost_weight = np.array([50.0, 50.0, 1.0, 20.0]), # weight for [x, y, yaw, v]\n",
    "    visualze_sampled_trajs = True, # if True, sampled trajectories are visualized\n",
    "    obstacle_circles = OBSTACLE_CIRCLES, # [obs_x, obs_y, obs_radius]\n",
    "    collision_safety_margin_rate = 1.2, # safety margin for collision check\n",
    ")\n",
    "\n",
    "# simulation loop\n",
    "for i in range(sim_steps):\n",
    "\n",
    "    # get current state of vehicle\n",
    "    current_state = vehicle.get_state()\n",
    "\n",
    "    try:\n",
    "        # calculate input force with MPPI\n",
    "        optimal_input, optimal_input_sequence, optimal_traj, sampled_traj_list = mppi.calc_control_input(\n",
    "            observed_x = current_state\n",
    "        )\n",
    "    except IndexError as e:\n",
    "        # the vehicle has reached the end of the reference path\n",
    "        print(\"[ERROR] IndexError detected. Terminate simulation.\")\n",
    "        break\n",
    "\n",
    "    # print current state and input force\n",
    "    print(f\"Time: {i*delta_t:>2.2f}[s], x={current_state[0]:>+3.3f}[m], y={current_state[1]:>+3.3f}[m], yaw={current_state[2]:>+3.3f}[rad], v={current_state[3]:>+3.3f}[m/s], steer={optimal_input[0]:>+6.2f}[rad], accel={optimal_input[1]:>+6.2f}[m/s]\")\n",
    "\n",
    "    # update states of vehicle\n",
    "    vehicle.update(u=optimal_input, delta_t=delta_t, optimal_traj=optimal_traj[:, 0:2], sampled_traj_list=sampled_traj_list[:, :, 0:2])\n",
    "\n",
    "# show animation\n",
    "vehicle.show_animation(interval_ms=int(delta_t * 1000))\n",
    "# save animation\n",
    "vehicle.save_animation(\"mppi_pathtracking_obav_demo.mp4\", interval=int(delta_t * 1000), movie_writer=\"ffmpeg\") # ffmpeg is required to write mp4 file"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
